#include <iostream>
#include <unistd.h>
#include <memory>
#include "dh_gripper_factory.h"
#include <unordered_map>

#include "ros/ros.h"
#include "dh_gripper_msgs/GripperCtrl.h"
#include "dh_gripper_msgs/GripperState.h"
#include "dh_gripper_msgs/GripperRotCtrl.h"
#include "dh_gripper_msgs/GripperRotState.h"

#include "sensor_msgs/JointState.h"


#include <boost/asio.hpp>
#include <boost/bind.hpp>
#include <boost/system/error_code.hpp>
#include <boost/asio/streambuf.hpp>



std::string _gripper_ID;
std::string _gripper_model;
std::string _gripper_connect_port;
std::string _gripper_Baudrate;
DH_Gripper *_gripper;



void update_gripper_control(const dh_gripper_msgs::GripperCtrl::ConstPtr& msg)
{
    
    if(msg->initialize)
    {
      _gripper->Initialization();  
    }
    else
    {
        ROS_INFO("speed:[%f],force: [%f], position: [%f]", msg->speed,msg->force, msg->position);
        _gripper->SetTargetSpeed((int)msg->speed);
        _gripper->SetTargetForce((int)msg->force);
        _gripper->SetTargetPosition((int)msg->position);
    }
}

void update_rotation_control(const dh_gripper_msgs::GripperRotCtrl::ConstPtr& msg)
{
    ROS_INFO("r_speed:[%f],r_force: [%f], r_angle: [%f]", msg->speed,msg->force, msg->angle);
    if(_gripper_model.find("RGI")!= _gripper_model.npos)
    {
        dynamic_cast<DH_RGI *>(_gripper)->SetTargetRotationTorque((int)msg->force); 
        dynamic_cast<DH_RGI *>(_gripper)->SetTargetRotationSpeed((int)msg->speed); 
        dynamic_cast<DH_RGI *>(_gripper)->SetTargetRotation((int)msg->angle); 

    }
    else if(_gripper_model.find("DH3_CAN")!= _gripper_model.npos)
    {
        dynamic_cast<DH_DH3_CAN *>(_gripper)->SetTargetRotation((int)msg->angle); 
    }

}


void update_gripper_state(dh_gripper_msgs::GripperState& msg)
{
    static long seq = 0;
    msg.header.stamp = ros::Time::now();
    msg.header.seq =seq; 
    int tmp_state[5] = {0};
    _gripper->GetRunStates(tmp_state);
    if(tmp_state[0] == 1)
        msg.is_initialized = true;
    else
        msg.is_initialized = false;
        
    msg.grip_state      = tmp_state[1];
    msg.position        = tmp_state[2];
    msg.target_position = tmp_state[3];
    msg.target_force    = tmp_state[4];
    seq++;
}

void update_gripper_joint_state(sensor_msgs::JointState& msg)
{
    static long seq = 0;
    msg.header.frame_id = "";
    msg.header.stamp = ros::Time::now();
    msg.header.seq = seq;
    
    msg.name.resize(1);
    msg.position.resize(1);


    int tmp_pos = 0;

    _gripper->GetCurrentPosition(tmp_pos);

    msg.position[0] = (1000-tmp_pos)/1000.0 * 0.637;
    msg.name[0] = "gripper_finger1_joint"; 

    seq++;
}



void update_rotation_state(dh_gripper_msgs::GripperRotState& msg)
{
    static long seq = 0;
    msg.header.stamp = ros::Time::now();
    msg.header.seq =seq; 
    int tmp_state[9] = {0};
    _gripper->GetRunStates(tmp_state); 
    msg.rot_state       = tmp_state[5];
    msg.angle           = tmp_state[6];
    msg.target_angle    = tmp_state[7];
    msg.target_force    = tmp_state[8];
    seq++;
}



class WebServer {
public:
    WebServer(int port)
        : io_service_(), acceptor_(io_service_, boost::asio::ip::tcp::endpoint(boost::asio::ip::tcp::v4(), port)),
          socket_(io_service_), response_() {
        
        
        ROS_WARN("port : %d", port);
        start_accept();
        io_service_.run();
    }

private:
    void start_accept() {
        acceptor_.async_accept(socket_,
            boost::bind(&WebServer::handle_accept, this,
                        boost::asio::placeholders::error));
    }

    void handle_accept(const boost::system::error_code& error) {
        if (!error) {
            boost::asio::async_read_until(
                socket_, response_, "\r\n\r\n",
                boost::bind(&WebServer::handle_read, this,
                            boost::asio::placeholders::error,
                            boost::asio::placeholders::bytes_transferred));
        } else {
            std::cerr << "Accept error: " << error.message() << std::endl;
            start_accept();
        }
    }

    void handle_read(const boost::system::error_code& error, size_t bytes_transferred) {
        if (error) {
            std::cerr << "Read error: " << error.message() << std::endl;
            socket_.close();
            start_accept();
            return;
        }

        std::string request_str = boost::asio::buffer_cast<const char*>(response_.data());
        request_str = request_str.substr(0, bytes_transferred);
        response_.consume(bytes_transferred);

        std::istringstream iss(request_str);
        std::string method, path, version;
        iss >> method >> path >> version; //http请求方法，路径，版本

        std::map<std::string, std::string> headers;
        std::string header_line;

        // 读取并解析所有请求头字段
        while (std::getline(iss, header_line)) {
            // 去除前后空白字符
            auto trim_left = std::find_if(header_line.begin(), header_line.end(),
                [](unsigned char ch) { return !std::isspace(ch); });
            if (trim_left == header_line.end()) {
                // 空行，结束头部解析
                break;
            }

            std::string trimmed_line(trim_left, header_line.end());

            size_t colon_pos = trimmed_line.find(':');
            if (colon_pos != std::string::npos) {
                std::string key = trimmed_line.substr(0, colon_pos);
                std::string value = trimmed_line.substr(colon_pos + 1);

                // 去除 value 前导空格
                value.erase(value.begin(), std::find_if(value.begin(), value.end(),
                    [](unsigned char ch) { return !std::isspace(ch); }));

                // key 转为小写
                std::transform(key.begin(), key.end(), key.begin(), ::tolower);

                headers[key] = value;
            }
        }

        // 获取 Content-Length
        size_t content_length = 0;
        auto it = headers.find("content-length");
        if (it != headers.end()) {
            std::string cl = it->second;
            cl.erase(std::remove_if(cl.begin(), cl.end(), [](char c) {
                return !std::isdigit(static_cast<unsigned char>(c));
            }), cl.end());

            if (!cl.empty()) {
                try {
                    content_length = std::stoull(cl);
                } catch (...) {
                    content_length = 0;
                }
            }
        }

        if (method == "POST" && path.find("/gripper_server") == 0) {
            if (content_length == 0) {
                ROS_WARN("test2");
                send_response("HTTP/1.1 400 Bad Request\r\n\r\n Empty body");
                return;
            }
            // 读取请求体
            boost::asio::async_read(socket_, response_, boost::asio::transfer_exactly(content_length),
                boost::bind(&WebServer::handle_post_body, this, _1, _2));
        } else {
            ROS_WARN("test1");
            send_response("HTTP/1.1 404 Not Found\r\n\r\n");
        }
    }

    void handle_post_body(const boost::system::error_code& error, size_t bytes_transferred) {
        
        int speed, force, position;


        if (error) {
            std::cerr << "Read body error: " << error.message() << std::endl;
            send_response("HTTP/1.1 500 Internal Server Error\r\n\r\n");
            return;
        }

        std::string body = boost::asio::buffer_cast<const char*>(response_.data());
        body = body.substr(0, bytes_transferred);
        response_.consume(bytes_transferred);

        // 解析表单数据
        std::unordered_map<std::string, std::string> params = parse_form_data(body);

        
        auto it_speed = params.find("speed");
        auto it_force = params.find("force");
        auto it_position = params.find("position");
        if (it_speed == params.end() || it_force == params.end() || it_position == params.end()) {
            send_response("HTTP/1.1 400 Bad Request\r\n\r\nMissing parameters");
            return;
        }

        std::string speed_str, force_str, position_str;
        speed_str = it_speed->second;
        force_str = it_force->second;
        position_str = it_position->second;

        speed = std::stod(speed_str);
        force = std::stod(force_str);
        position = std::stod(position_str);

        ROS_WARN("speed:[%d],force: [%d], position: [%d]", speed, force, position);

        //发送控制命令
        // _gripper->SetTargetSpeed(speed);
        // _gripper->SetTargetForce(force);
        // _gripper->SetTargetPosition(position);


        // 构造 JSON 响应
        std::ostringstream oss;
        oss << R"({"status":"success","params":{)";
        oss << R"("speed":")" << speed << R"(",)";
        oss << R"("force":")" << force << R"(",)";
        oss << R"("position":")" << position << R"("}})";
        std::string response = "HTTP/1.1 200 OK\r\n"
                               "Content-Type: application/json\r\n"
                               "Content-Length: " + std::to_string(oss.str().size()) + "\r\n"
                               "Connection: close\r\n"
                               "\r\n" + oss.str();

        send_response(response);
    }

    void send_response(const std::string& response) {
        boost::asio::async_write(
            socket_,
            boost::asio::buffer(response),
            [this](const boost::system::error_code&, size_t) {
                socket_.close();
                start_accept();
            });
    }

    std::unordered_map<std::string, std::string> parse_form_data(const std::string& body) {
        std::unordered_map<std::string, std::string> params;
        std::istringstream iss(body);
        std::string pair;

        while (std::getline(iss, pair, '&')) {
            size_t eq_pos = pair.find('=');
            if (eq_pos != std::string::npos) {
                std::string key = pair.substr(0, eq_pos);
                std::string value = pair.substr(eq_pos + 1);
                params[key] = value;
            }
        }

        return params;
    }

    boost::asio::io_service io_service_;
    boost::asio::ip::tcp::acceptor acceptor_;
    boost::asio::ip::tcp::socket socket_;
    boost::asio::streambuf response_;
    boost::asio::streambuf request_body_buffer_;
};



int main(int argc, char** argv)
{
    ros::init(argc, argv, "dh_gripper_driver");
    ros::NodeHandle n("~");

    n.param<std::string>("Gripper_ID", _gripper_ID,"1");
    n.param<std::string>("Gripper_Model", _gripper_model,"AG95_MB");
    n.param<std::string>("Connect_port", _gripper_connect_port,"/dev/ttyUSB0");
    n.param<std::string>("BaudRate", _gripper_Baudrate, "115200");
    
    ROS_INFO("Gripper_ID : %s", _gripper_ID.c_str());
    ROS_INFO("Gripper_model : %s", _gripper_model.c_str());
    ROS_INFO("Connect_port: %s", _gripper_connect_port.c_str());
    ROS_INFO("BaudRate : %s (In TCP/IP Mode , BaudRate is unuse)", _gripper_Baudrate.c_str());



    // std::unique_ptr<WebServer> web_server = std::make_unique<WebServer>(8080); //创建web服务
    WebServer web_server(12300);

    ros::spin();
    
    std::unique_ptr<DH_Gripper_Factory> _gripper_Factory = std::make_unique<DH_Gripper_Factory>();
    _gripper_Factory->Set_Parameter(atoi(_gripper_ID.c_str()), _gripper_connect_port, atoi(_gripper_Baudrate.c_str()));
    

    _gripper = _gripper_Factory->CreateGripper(_gripper_model);
    if(_gripper == NULL)
    {
        ROS_ERROR("No this Model :%s", _gripper_model.c_str());
        return -1;
    }   

     
    if(_gripper->open()<0)
    {
        ROS_ERROR("Unable to open commport to %s", _gripper_connect_port.c_str());
        return -1;
    }

    //initialize the gripper
    int initstate = 0;
    _gripper->GetInitState(initstate);
    if(initstate != DH_Gripper::S_INIT_FINISHED)
    {
        _gripper->Initialization();
        std::cout<< " Send grip init " << std::endl;

        //wait for gripper initialization
        initstate = 0;
        std::cout<< " wait grip initialized " << std::endl;
        while(initstate != DH_Gripper::S_INIT_FINISHED )
            _gripper->GetInitState(initstate); 
        std::cout<< "GetInitState "<< initstate << std::endl;
    }

    ros::Subscriber _sub_grip ;
    ros::Publisher _gripper_state_pub;
    ros::Subscriber _sub_rot ;
    ros::Publisher _rot_state_pub;

    _sub_grip = n.subscribe("/gripper/ctrl", 50, update_gripper_control);
    _gripper_state_pub = n.advertise<dh_gripper_msgs::GripperState>("/gripper/states", 50);

    if(_gripper->GetGripperAxiNumber()==2)
    {
        _sub_rot = n.subscribe("/gripper/rot_ctrl", 50, update_rotation_control);
        _rot_state_pub = n.advertise<dh_gripper_msgs::GripperRotState>("/gripper/rot_states", 50);
    }
    //TODO
    ros::Publisher _gripper_joint_state_pub = n.advertise<sensor_msgs::JointState>("/gripper/joint_states", 50); 

    ros::Rate loop_rate(50);
     while (ros::ok())
     {

         dh_gripper_msgs::GripperState msg_g_state;
         update_gripper_state(msg_g_state);
        _gripper_state_pub.publish(msg_g_state);

        sensor_msgs::JointState msg_g_joint_state;
        update_gripper_joint_state(msg_g_joint_state);
        _gripper_joint_state_pub.publish(msg_g_joint_state);

        if(_gripper->GetGripperAxiNumber()==2)
        {
            dh_gripper_msgs::GripperRotState msg_r_state;
            update_rotation_state(msg_r_state);
            _rot_state_pub.publish(msg_r_state);

        }
  
        ros::spinOnce();
  
        loop_rate.sleep();
    }


    _gripper->close();


    return 0;

}